/*  
 *  Nanjing JIYU Technology Co.,Ltd All rights reserved.
 *  Copyright (C) 2022 
 *  ==============================================================
 * 
 *  gray.c 
 *    灰度传感器功能的实现
 *
 *  --------------------------------------------------------------
 *  
 */

#include "gray.h"

/*------------------------ 通过灰度判断位置偏差 --------------------*/

/* 前进时候的位置偏差 */
signed char gray_front_error(void)
{
  u16 temp = (GRAY_F_L << 5) | (GRAY_F_M << 4) | (GRAY_F_R<<3) | (GRAY_B_L << 2) | (GRAY_B_M << 1) | GRAY_B_R;   //灰度拼接起来
  signed char err;    /* 位置偏差   白线为0，黑线为1,err负为向左偏，正为向右偏 */

  switch (temp)
  {
    case 0x1F:          // 011111  车身偏右++，需要向左偏 
      err = -5;//-4
      break;
    case 0x2F:          // 101111 车身偏右+，需要向左偏
      err = -4;//-3
      break;
    case 0x37:          // 110111 车身偏右，需要向左偏
      err = -3;//-2
      break;
    case 0x33:          // 110011 正中间
      err = 0;//0
      break;
		case 0x3B:					//111011 车身偏左，需要往右偏
			err = 2;//2
		  break;
		case 0x3D:  				//111101 车身偏左+，需要向右偏
			err = 3;//3
		  break;
		case 0x3E:					//111110 车身偏右++，需要向右偏
			err = 4;//4
		  break;
    default:            // 101 白线在中间，111 跑出去了
      err = 0;//0
      break;
  }
  return err;
}

/* 后退时候的位置偏差 */
signed char gray_back_error(void)
{
  u16 temp = (GRAY_F_L << 5) | (GRAY_F_M << 4) | (GRAY_F_R<<3) | (GRAY_B_L << 2) | (GRAY_B_M << 1) | GRAY_B_R;   //灰度拼接起来
  signed char err;    /* 位置偏差   白线为0，黑线为1,err负为向左偏，正为向右偏 */

  switch (temp)
  {
    case 0x1F:          // 011111  车身偏右++，需要向左偏 
      err = -3;//-3
      break;
    case 0x2F:          // 101111 车身偏右+，需要向左偏
      err = -2;//-2
      break;
    case 0x37:          // 110111 车身偏右，需要向左偏
      err = -1;//-1
      break;
    case 0x33:          // 110011 正中间
      err = 0;//0
      break;
		case 0x3B:					//111011 车身偏左，需要往右偏
			err = 1;//1
		  break;
		case 0x3D:  				//111101 车身偏左+，需要向右偏
			err = 2;//2
		  break;
		case 0x3E:					//111110 车身偏右++，需要向右偏
			err = 3;//3
		  break;
    default:            // 101 白线在中间，111 跑出去了
      err = 0;//0
      break;
  }
  return err;
}

/* 判断是否是十字路口或十字标线 */
bool gray_is_cross(void)
{
	
  return 
      (
       (GRAY_R_F && GRAY_R_M && GRAY_R_B) 	||
      ( (GRAY_F_L && GRAY_F_M && GRAY_F_R)	&&
			(GRAY_B_L && GRAY_B_M && GRAY_B_R) ) 	||
			 (GRAY_L_F && GRAY_L_M && GRAY_L_B)
			) == 0; 
}
bool gray_is_back(void)
{
	return 
      (
       (GRAY_R_F && GRAY_R_M && GRAY_R_B) 	
	//     ||
			// (GRAY_L_F && GRAY_L_M && GRAY_L_B)
			) == 0; 
	//左右方向都至少有一个灰度值为0
}
/* 判断是否为T字路口 */
bool gray_is_t(void)
{
  return (!(GRAY_F_L && GRAY_F_M && GRAY_F_R) ||
          ((GRAY_L_F && GRAY_L_M && GRAY_L_B) ||
           (GRAY_R_F && GRAY_R_M && GRAY_R_B)) ||
           !(GRAY_B_L && GRAY_B_M && GRAY_B_R)) == 0;
	//除去前方，另三个方向至少各有一个方向的灰度不为零
}

/* 判断转弯是否到位，可以直接用十字路口逻辑，这个条件更宽松 */
bool gray_turn_not_ok0(void)
{
  /* 车头对正白线，转弯就可以停止 */
  return (GRAY_F_R && GRAY_B_L);
}

void gray_test(void)
{

  while (1)
  {
    if (gray_is_cross() == 1)
    {
      printf("cross\n");
    }
    if (gray_is_t() == 1)
    {
      printf("T\n");
    }
    if (gray_turn_not_ok0() == 0)
    {
      printf("ok0\n");
    }
		
    delay_ms(20);
    printf(".\n");
  }
}
int speed_plus(int speed)
{

	if (fabs(gray_back_error()) < 2 && speed <600)
	{
		speed+=1;
	}
		if (fabs(gray_back_error()) < 2 && speed ==600)
		{
			speed =600;
		}
	else speed = 350;
	
	return speed;
}